#ifndef GEO_H_
#define GEO_H_

#include <stdint.h>
#include <math.h>
#include <vector>

#ifndef INT32_MAX
#define INT32_MAX 0x7fffffffL
#define INT32_MIN (-INT32_MAX - 1L)
#endif

#define EARTH_RADIUS 6371000

#define DEGTOMAPUNIT(x) ((int32_t)((x * (4294967296.0 / 360.0))))
#define MAPUNITTODEG(x) ((double)x * (360.0 / 4294967296.0))

using namespace std;

namespace opennav {

struct Coordinate {
	Coordinate(int32_t lon, int32_t lat) :
		longitude(lon), latitude(lat) {
	}
	Coordinate() :
		longitude(0), latitude(0) {
	}
	Coordinate(double lon, double lat) :
		longitude(DEGTOMAPUNIT(lon)), latitude(DEGTOMAPUNIT(lat)) {
	}

	int32_t longitude;
	int32_t latitude;

	double dlongitude() const {
		return MAPUNITTODEG(longitude);
	}

	double dlatitude() const {
		return MAPUNITTODEG(latitude);
	}

	int distanceTo(const Coordinate& c) {
		const float aLat = rad(intToDoubleCoordinate(latitude));
		const float bLat = rad(intToDoubleCoordinate(c.latitude));
		const float aLon = rad(intToDoubleCoordinate(longitude));
		const float bLon = rad(intToDoubleCoordinate(c.longitude));
		return (EARTH_RADIUS * acos(sin(aLat) * sin(bLat) + cos(aLat) * cos(
				bLat) * cos(bLon - aLon)));
	}

	inline float intToDoubleCoordinate(const int value) {
		return (float) (value * (360.0f / 16777216.0f));
	}

	inline float rad(const float degree) {
		return (degree * 3.1415926) / 180.0;
	}

	bool operator==(const Coordinate c) const {
		return longitude == c.longitude && latitude == c.latitude;
	}

	bool operator==(const Coordinate* c) const {
		return longitude == c->longitude && latitude == c->latitude;
	}
};

struct Area {
	typedef int OutCode;
#define INSIDE 0
#define LEFT 1
#define RIGHT 2
#define BOTTOM 4
#define TOP 8

	Area() :
		min_longitude(INT32_MAX), min_latitude(INT32_MAX), max_longitude(
				INT32_MIN), max_latitude(INT32_MIN) {
	}
	Area(const Coordinate& center, int32_t width, int32_t height) :
		min_longitude(center.longitude - width / 2), min_latitude(
				center.latitude - height / 2), max_longitude(center.longitude
				+ width / 2), max_latitude(center.latitude + height / 2) {
	}

	int32_t min_longitude;
	int32_t min_latitude;
	int32_t max_longitude;
	int32_t max_latitude;

	void add_to_bounds(const Coordinate& coordinate) {
		if (min_longitude > coordinate.longitude)
			min_longitude = coordinate.longitude;
		if (min_latitude > coordinate.latitude)
			min_latitude = coordinate.latitude;
		if (max_longitude < coordinate.longitude)
			max_longitude = coordinate.longitude;
		if (max_latitude < coordinate.latitude)
			max_latitude = coordinate.latitude;
	}
	void add_to_bounds(const vector<Coordinate>& coordinates) {
		for (vector<Coordinate>::const_iterator it = coordinates.begin(); it
				!= coordinates.end(); it++) {
			if (min_longitude > (*it).longitude)
				min_longitude = (*it).longitude;
			if (min_latitude > (*it).latitude)
				min_latitude = (*it).latitude;
			if (max_longitude < (*it).longitude)
				max_longitude = (*it).longitude;
			if (max_latitude < (*it).latitude)
				max_latitude = (*it).latitude;
		}
	}
	bool intersects(const Area& area) const {
		if (area.max_longitude < min_longitude || area.min_longitude
				> max_longitude || area.max_latitude < min_latitude
				|| area.min_latitude > max_latitude)
			return false;
		else
			return true;
	}
	bool contains(const Coordinate& coordinate) const {
		if (max_longitude < coordinate.longitude || min_longitude
				> coordinate.longitude || max_latitude < coordinate.latitude
				|| min_latitude > coordinate.latitude)
			return false;
		else
			return true;
	}

	OutCode computeOutCode(const Coordinate& coord) const {
		OutCode code = INSIDE;

		if (coord.longitude < min_longitude){
			code |= LEFT;
		}
		else if (coord.longitude > max_longitude){
			code |= RIGHT;
		}
		if (coord.latitude < min_latitude){
			code |= BOTTOM;
		}
		else if (coord.latitude > max_latitude){
			code |= TOP;
		}
		return code;
	}

	bool containsLine(Coordinate start, Coordinate end) const {
		// Cohen-Sutherland - adapted from wikipedia

		OutCode outcode0 = computeOutCode(start);
		OutCode outcode1 = computeOutCode(end);

		while (true) {
			if (!(outcode0 | outcode1)) { //logical or is 0. Trivially accept and get out of loop
				return true;
			} else if (outcode0 & outcode1) { //logical and is not 0. Trivially reject and get out of loop
				return false;
			} else {
				int32_t x = 0, y = 0;

				OutCode outcodeOut = outcode0 ? outcode0 : outcode1;

				int32_t dx = end.longitude - start.longitude;
				int32_t dy = end.latitude - start.latitude;

				if (outcodeOut & TOP) {
					x = start.longitude + (max_latitude - start.latitude) * dx / dy;
					y = max_latitude;
				} else if (outcodeOut & BOTTOM) {
					x = start.longitude + (min_latitude - start.latitude) * dx / dy;
					y = min_latitude;
				} else if (outcodeOut & RIGHT) {
					y = start.latitude + (max_longitude - start.longitude) * dy / dx;
					x = max_longitude;
				} else if (outcodeOut & LEFT) {
					y = start.latitude + (min_longitude - start.longitude) * dy / dx;
					x = min_longitude;
				}

				if (outcodeOut == outcode0) {
					start.longitude = x;
					start.latitude = y;
					outcode0 = computeOutCode(start);
				} else {
					end.longitude = x;
					end.latitude = y;
					outcode1 = computeOutCode(end);
				}
			}
		}
	}
};

}

#endif /* GEO_H_ */
